module cordic_demod (
	input clk,
	input resetn,

	input s_axis_valid,
	output s_axis_ready,
	input [63:0] s_axis_data,

	output m_axis_valid,
	input m_axis_ready,
	output [63:0] m_axis_data
);

reg [4:0] step_counter;
reg [4:0] shift_counter;
reg [30:0] phase;
reg [2:0] state;

reg [32:0] i;
reg [32:0] q;
reg [32:0] i_shift;
reg [32:0] q_shift;

assign s_axis_ready = state == STATE_IDLE;
assign m_axis_data = {q[32:1],i[32:1]};
assign m_axis_valid = state == STATE_DONE;

localparam STATE_IDLE = 0;
localparam STATE_SHIFT_LOAD = 1;
localparam STATE_SHIFT = 2;
localparam STATE_ADD = 3;
localparam STATE_DONE = 4;

reg [31:0] angle[0:30];

initial begin
	angle[0] = 32'h20000000;
	angle[1] = 32'h12e4051e;
	angle[2] = 32'h09fb385b;
	angle[3] = 32'h051111d4;
	angle[4] = 32'h028b0d43;
	angle[5] = 32'h0145d7e1;
	angle[6] = 32'h00a2f61e;
	angle[7] = 32'h00517c55;
	angle[8] = 32'h0028be53;
	angle[9] = 32'h00145f2f;
	angle[10] = 32'h000a2f98;
	angle[11] = 32'h000517cc;
	angle[12] = 32'h00028be6;
	angle[13] = 32'h000145f3;
	angle[14] = 32'h0000a2fa;
	angle[15] = 32'h0000517d;
	angle[16] = 32'h000028be;
	angle[17] = 32'h0000145f;
	angle[18] = 32'h00000a30;
	angle[19] = 32'h00000518;
	angle[20] = 32'h0000028c;
	angle[21] = 32'h00000146;
	angle[22] = 32'h000000a3;
	angle[23] = 32'h00000051;
	angle[24] = 32'h00000029;
	angle[25] = 32'h00000014;
	angle[26] = 32'h0000000a;
	angle[27] = 32'h00000005;
	angle[28] = 32'h00000003;
	angle[29] = 32'h00000001;
	angle[30] = 32'h00000001;
end

always @(posedge clk) begin
	if (resetn == 1'b0) begin
		state <= STATE_IDLE;
	end else begin
		case (state)
		STATE_IDLE: begin
			if (s_axis_valid == 1'b1) begin
				state <= STATE_SHIFT_LOAD;
			end
		end
		STATE_SHIFT_LOAD: begin
			if (step_counter == 'h00) begin
				state <= STATE_ADD;
			end else begin
				state <= STATE_SHIFT;
			end
		end
		STATE_SHIFT: begin
			if (shift_counter == 'h01) begin
				state <= STATE_ADD;
			end
		end
		STATE_ADD: begin
			if (step_counter == 'd30) begin
				state <= STATE_DONE;
			end else begin
				state <= STATE_SHIFT_LOAD;
			end
		end
		STATE_DONE: begin
			if (m_axis_ready == 1'b1)
				state <= STATE_IDLE;
		end
		endcase
	end
end

always @(posedge clk) begin
	case(state)
	STATE_SHIFT_LOAD: begin
		shift_counter <= step_counter;
	end
	STATE_SHIFT: begin
		shift_counter <= shift_counter - 1'b1;
	end
	endcase
end

always @(posedge clk)
begin
	case(state)
	STATE_IDLE:
		if (s_axis_valid == 1'b1) begin
			step_counter <= 'h00;
			phase <= {1'b0,s_axis_data[61:32]};
			step_counter <= 'h00;
			case (s_axis_data[63:62])
			2'b00: begin
				i <= {s_axis_data[31],s_axis_data[31:0]};
				q <= 'h00;
			end
			2'b01: begin
				i <= 'h00;
				q <= ~{s_axis_data[31],s_axis_data[31:0]};
			end
			2'b10: begin
				i <= ~{s_axis_data[31],s_axis_data[31:0]};
				q <= 'h00;
			end
			2'b11: begin
				i <= 'h00;
				q <= {s_axis_data[31],s_axis_data[31:0]};
			end
			endcase
		end
	STATE_SHIFT_LOAD: begin
		i_shift <= i;
		q_shift <= q;
	end
	STATE_SHIFT: begin
		i_shift <= {i_shift[32],i_shift[32:1]};
		q_shift <= {q_shift[32],q_shift[32:1]};
	end
	STATE_ADD: begin
		if (phase[30] == 1'b0) begin
			i <= i + q_shift;
			q <= q - i_shift;
			phase <= phase - angle[step_counter];
		end else begin
			i <= i - q_shift;
			q <= q + i_shift;
			phase <= phase + angle[step_counter];
		end
		step_counter <= step_counter + 1'b1;
	end
	endcase
end

endmodule
